//Here's the documentation for the AccelStepper file
//http://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html
#include <AccelStepper.h>
#include <iostream>
#include <string>
// include the LCD library code:
#include "Wire.h"
#include "LiquidCrystal.h"


const int StationOne = 500;
//const int StationTwo = 500;
//const int StationThree = 500;
//const int StationFour = 500;
//const int StationFive = 500;
//const int StationSix = 500;





////////////////////////////////////////////////////////////////////////////////////////////////////////
//initialize Stepper Motor
////////////////////////////////////////////////////////////////////////////////////////////////////////
int motorSpeed = 9000; //maximum steps per second (about 3rps / at 16 microsteps)
int motorAccel = 10000; //steps/second/second to accelerate

int motorDirPin = 5; //digital pin 2
int motorStepPin = 4; //digital pin 3

int movementAmt;
int movementDir = 2;  //1 = right, 2 = left
//set up the accelStepper intance
//the "1" tells it we are using a driver
AccelStepper stepper(1, motorStepPin, motorDirPin); 


//========================================================================================================

//////////////////////////////////////////////////////////////////////////////
//Initialize Limit switch sensor
//////////////////////////////////////////////////////////////////////////////
int limitSwitchSensorLeft = 5;
int limitSwitchSensorRight = 4;

volatile int RightSensor;   //
volatile int LeftSensor;    // 

//////////////////////////////////////////////////////////////////////////////

//========================================================================================================


////////////////////////////////////////////////////////////////////////////////////////////////////////
//initialize LCD
////////////////////////////////////////////////////////////////////////////////////////////////////////



//LiquidCrystal lcd(0);






//////////////////////////////////////////////////////////////////////////////

//========================================================================================================

//Initialize Cup sensor
int weightSensorPin = 0;        // select the input pin for the Weight Sensor
double weightThreshold = .10;   // select the cutoff for sensing a cup

//use this as a global variable to keep track of arduino's current goal.  when arduino receives serial command 
//from the computer, it will be in this string format "1429,1,1"
//in order to convert the string into a usable format, use the "ParseSerial(string) command to update the current 
//goal.  when goal is complete, will set all values to -1.

String CurrentCommands[3];

void setup() {
  
  // Initialize Inturupt
  attachInterrupt(1, IntLeftSwitch, RISING);   // 3 = LEFT
  attachInterrupt(0, IntRightSwitch, RISING);   // 2 = right
  
  RightSensor = LOW;   //
  LeftSensor = LOW; 
  
  //pretend this is a serial communication:
//////////////////////  String serialComOne[3] = {"49000","1","2"};
//////////////////////  String serialComTwo[3] = {"35000","0","1"};
  
  
  //Setup Variables for Program
  CurrentCommands[0] = "-1";        //StationID or location
  CurrentCommands[1] = "-1";        //DrinkType
  CurrentCommands[2] = "-1";        //Qty
  
  //Setup stepper motor  
  stepper.setMaxSpeed(motorSpeed);
  stepper.setSpeed(motorSpeed);
  stepper.setAcceleration(motorAccel);
  
  //Have it move in specific direction until it hits a switch.  at which point this will turn off, and reset the starting point of the stepper motor position
  //stepper.moveTo(32000); //move 32000 steps (should be 10 rev)

  //Intallize Limit switch sensor  
  //pinMode(limitSwitchSensorLeft,INPUT);
  //pinMode(limitSwitchSensorRight,INPUT);

  /*
   INPUTS NEEDED TO SELECT DRINK:
   - StationID
   - DrinkType  (effects the use of QTY)
   - QTY
   
   Sample set:
   "49000,1,2"  = "move to station 49000, dispense two oz of mixer (because drinktype 1 = mixer)"
   "35000,0,1"  = "move to station 35000, dispense one shot spirit (becasuse drinktype 2 = spirit)"   
   */

  //lcd.begin(16, 2);
  //Serial.begin(4800);
  
  InitializeSteppermotor();
  movementDir = 2;
  
  stepper.moveTo(-85000);
}

void loop() {
  //int movementDir = 1;  //1 = right, 2 = left
  
  if (movementDir == 1)
  {
    //lcd.setCursor(0, 0);
    //lcd.print("Moving to Left");
    stepper.moveTo(85000);
  }
  else
  {
    //lcd.setCursor(0, 0);
    //lcd.print("Moving to Right");
    stepper.moveTo(-85000);
  }
  
  //for (int x = 0; x < 15 ; x++)
  //{    
 //   stepper.run();
 //   delay(5); 
 // }
 
   
  while(RightSensor == LOW && LeftSensor == LOW  )
  {    
    stepper.run();   
  }
  
  stepper.moveTo(stepper.currentPosition());
  
  if (LeftSensor == HIGH )
  {
    movementDir = 1;
    
    //lcd.setCursor(3, 1);
    //lcd.print("Hit-L");
    
    //Serial.print("Hit left side.  Changing movementDir to 1 at position: ");
    //Serial.println(stepper.currentPosition());
  }  
  
  if (RightSensor == HIGH )
  {
    movementDir = 2;
    //lcd.setCursor(3, 1);
    //lcd.print("Hit-R");
    
    //Serial.println("Hit right side.  Changing movementDir to 2 at position: ");
    //Serial.println(stepper.currentPosition());
  }
  ResetInterruptVars(); 
}

void ParseSerial(String serialData)
{
  String StationID = "15000", DrinkType = "0", Qty = "2";  
  
  //parse the data, and pass the pieces into "CurrentCommands"
  CurrentCommands[0] = StationID;  //StationID or location
  CurrentCommands[1] = DrinkType;  //DrinkType
  CurrentCommands[2] = Qty;        //Qty
  
  //For debugging purposes: print the result
  Serial.print("Station: ");
  Serial.println(CurrentCommands[0]);
  Serial.print("Drink Type: ");
  Serial.println(CurrentCommands[1]);  
  Serial.print("Qty: ");
  Serial.println(CurrentCommands[2]);
}


boolean CheckIfSensesCup()
{    
    while( weightThreshold > analogRead(weightSensorPin) )
    {
        Serial.println("Waiting for cup...");
        UpdateState("Waiting for Cup");
        delay(500);           
    }
  
    Serial.println("");
    Serial.println("Cup registered");  
}

String UpdateState(String updateText, int Red, int Green, int Blue)
{
   //Serial.print("Update State: ");
   //Serial.println(updateText); 
   //Write code for updating the RGB backlights
}
String UpdateState(String updateText)
{
   //Serial.print("Update State: ");
   //Serial.println(updateText);
}
String UpdateState(int Red, int Green, int Blue)
{
   //Write code for updating the RGB backlights
}


void InitializeSteppermotor()
{
  //lcd.setCursor(0, 0);
  //lcd.print("Initializing...");
  //UpdateState("Initializing stepper motor");
  stepper.moveTo(320000);
  
  //Begin the startup process
  while(RightSensor == LOW && LeftSensor == LOW )
  {    
    stepper.run();   
  }
  
  //Serial.println("Hit right side.  Setting current location to zero point.");
  //stepper.moveTo(stepper.currentPosition());
  
  //sets the current position as zero
  stepper.setCurrentPosition(stepper.currentPosition()); 
  ResetInterruptVars(); 
}



/* returns 0 if no switch is triggered, 1 if Right, 2 if Left
int CheckLimitSwitches()
{
    UpdateSwitchVariables();
    //lcd.setCursor(0, 1);
    
    if (LeftSensor == HIGH)
    {
        //UpdateState("LEFT LIMIT SWITCH IS TRIGGERED!");
        //lcd.print("2");
        return 2;      
    }    
    else if (RightSensor == HIGH)
    {
        //UpdateState("RIGHT LIMIT SWITCH IS TRIGGERED!");
        //lcd.print("1");
        return 1;      
    }
    else
    {
       //lcd.print("0");
       return 0;    // stands for "Clear!"
    }
}
*/


void ResetInterruptVars()
{
    RightSensor = LOW;  
    LeftSensor = LOW; 
}


//Interrupts!!!!
void IntRightSwitch()
{
    RightSensor = HIGH;  
}
void IntLeftSwitch()
{
    LeftSensor = HIGH;  
}

/* 

FUTURE USE:
SERIAL COMMUNICATION

  String serialDataIn;
  String data[3];
  int counter;
  int inbyte;
  counter = 0;
  serialDataIn = String("");
  inbyte = 0;
  
  if(Serial.available()){
    inbyte = Serial.read();
    if(inbyte >= '0')
      serialDataIn += inbyte;
    if (inbyte == ','){  // Handle delimiter
      data[counter] = String(serialDataIn);
      serialDataIn = String("");
      Serial.println(data[counter]);
      counter = counter + 1;
    }
    if(inbyte ==  '\r'){  // end of line
      //handle end of line a do something with data
      Serial.println("    - end transmission");
    }        
  }
  
*/

